#include "control.h"

float kp = -0.001, ki = -0.00000, kd = -0.003;
int brake_time = 100;
char* phone_num = "17802581199";

int bottle_cnt=0, fork_cnt=0, gear_cnt=0, toy_cnt=0;

void controld(void *arg){
    /*检测目标物体*/
    const int8_t *item = (int8_t*)(serial_data+1), *value = (int8_t*)(serial_data+2);
    int8_t alarm_cooldown = 0;
    enum items{BOTTLE=2, BOX=3, FORK=4, GEAR=5, TOY=6};

    const int8_t *rho_err = (int8_t*)(serial_data+3), *theta_err = (int8_t*)(serial_data+4), *magnitude = (int8_t*)(serial_data+5);
    int err, olderr, interr, differr, output;
    rt_tick_t curr_time, old_time, diff_time;

    while (1){
        /* 如果找到目标物体则报警 */
        if (alarm_cooldown == 0){
            if (*value>70 && *item!=BOX){
                switch (*item){
                case BOTTLE:
                    rt_mb_send(alarms, 0xff00f0f0);
                    ++bottle_cnt;
                    break;
                case FORK:
                    rt_mb_send(alarms, 0xffff0000);
                    ++fork_cnt;
                    break;
                case GEAR:
                    rt_mb_send(alarms, 0xff00ff00);
                    ++gear_cnt;
                    break;
                case TOY:
                    rt_mb_send(alarms, 0xf0f0f0f0);
                    ++toy_cnt;
                    break;
                }
                alarm_cooldown = 20;
            }
        }else{
            --alarm_cooldown;
        }

        /* 执行遥控程序或自动控制 */
        int ch1, ch2, ch3, ch4, ch6;
        ch6 = sbus_read_channel(6);
        if (ch6 > SBUS_MID){
            /*执行遥控 */
            ch1 = sbus_read_channel(1);
            ch2 = sbus_read_channel(2);
            ch3 = sbus_read_channel(3);
            ch4 = sbus_read_channel(4);
            /*一副翼 二升降 三油门 四方向*/
            servo_write(&esc_left, sbus2servo(restirct(ch3+ch4-1000, 1800, 200)));
            servo_write(&esc_right, sbus2servo(restirct(ch3-ch4+1000, 1800, 200)));
            servo_write(&esc_back, sbus2servo(ch1));
            if (ch2 > 1200){
                float_up();
            }else if (ch2 < 800){
                dive_down();
            }else{
                hang();
            }
        }else{
            /*PID循迹*/
            if (*magnitude < MAGNITUDE_THRESH){
                /*没有发现明显的直线则停止*/
                servo_write(&esc_left, SERVO_MID);
                servo_write(&esc_right, SERVO_MID);
                servo_write(&esc_back, SERVO_MID);
                hang();

                /* 停止后重置PID控制 */
                err = 0;
                differr = 0;
                interr = 0;
            }else{
                curr_time = recieve_time;
                if (curr_time != old_time){
                    /* 执行PID控制 */
                    diff_time = curr_time - old_time;
                    old_time = curr_time;

                    olderr = err;
                    err = *theta_err + *rho_err;
                    err = restirct(err, 30, -30);
                    if (err > 0){
                        err = 15 - (30-err)*(30-err)/60;
                    }else{
                        err = (30+err)*(30+err)/60 - 15;
                    }
                    differr = err - olderr;
                    interr += err;
                    output = SERVO_HALF * (kp*err + ki*interr + kd*differr);

                    servo_write(&esc_left, DEFAULT_SPEED - output);
                    servo_write(&esc_right, DEFAULT_SPEED + output);

                    rt_kprintf("theta:%d rho:%d err:%d differr:%d interr:%d output:%d olderr:%d difftime:%d\r\n", *theta_err, *rho_err, err, differr, interr, output*1000/SERVO_HALF, olderr, diff_time);
                }
            }
        }

        rt_thread_mdelay(50);
    }
}


void messaged(void *arg){
    static int msg_sent = 0;
    uint16_t ch5;
    while (1){
        ch5 = sbus_read_channel(5);
        if (ch5 > (SBUS_MID+SBUS_MAX)/2){
            if (!msg_sent){
                char msg[128];
                sprintf(msg, "%d bottles, %d forks, %d gears and %d toys found.", bottle_cnt, fork_cnt, gear_cnt, toy_cnt);
                send_text(phone_num, msg);
                msg_sent = 1;
            }
        }else if (ch5 < (SBUS_MID+SBUS_MIN)/2){
            if (msg_sent){
                msg_sent = 0;
                /* 重置计数器 */
                bottle_cnt = fork_cnt = gear_cnt = toy_cnt = 0;
            }
        }
        rt_thread_mdelay(1000);
    }
}

//void pid(int argc, char **argv){
//    if (argc >= 4){
//        kp = atoff(argv[1]);
//        ki = atoff(argv[2]);
//        kd = atoff(argv[3]);
//    }
//}


//MSH_CMD_EXPORT(pid, adjust pid)
